/*
 **********************************************************************
 * File name: cpf_att.cpp
 *
 * COPYRIGHT (C) 2018-2022
 *
 * The license and distribution terms for this file may be found in 
 * the LICENSE file.
 *
 * Author: ChenLong (Add new editor)
 * Email: worldlong@foxmail.com
 **********************************************************************
*/
#include "cpf_att.h"
/*
*/
CPF_Att::CPF_Att() :
    q(1.0f, 0, 0, 0),
    accel_initialized(false),
    magn_initialized(false),
    _counter(0),
    _accel_sum(0),
    _magn_sum(0),
    _w_b{0},
    _w_e{0},
    _ka(0.1),
    _km(0.01),
    _kg(0.1)
  {
  }
/*
*/
bool CPF_Att::initialize_with_accel(Vector3f accel)
{
  _accel_sum -= accel;
  
  if(++_counter == 100) {
    _accel_sum /= 100;
    _accel_sum.normalize();
    Vector3f vg(0, 0, 1.0f);
    Vector3f axis = _accel_sum.cross(vg);
    float angle = asinf(axis.norm()) / 2.0f;
    float cos_angle = cosf(angle);
    float sin_angle = sinf(angle);
    axis = axis.normalized() * sin_angle;
    q = Quaternion(cos_angle, axis);
    _accel_sum.set_zero();
    _counter = 0;
    accel_initialized = true;
    return true;
  }
  
  return false;
}
/*
*/
bool CPF_Att::initialize_with_magn(Vector3f magn)
{
  return false;
}
/*
*/
void CPF_Att::predict_with_gyro(Vector3f gyro, float dt)
{
  /*----------------------------*
   *   we = w - wb              *
   *----------------------------*/
  _w_e = gyro - _w_b; 
  
  /*----------------------------*
   *   q = dq*q                 *
   *----------------------------*/
  Quaternion dq(1, _w_e*(dt*0.5f));
  q = dq*q;
  q.normalize();
}

/*
*/
void CPF_Att::correct_with_accel(Vector3f accel, float dt)
{
  Vector3f vm = -accel.normalized();
  Vector3f vr(2*(q.q1*q.q3 - q.q0*q.q2),
              2*(q.q0*q.q1 + q.q2*q.q3),
              (1 - 2*q.q1*q.q1 - 2*q.q2*q.q2));

  /*-------------------------------------
   *  The axis of correction rotation
   *         vc = vm X vr
   *         vc = vc / |vc|
   *-------------------------------------*/
  Vector3f vc = vm.cross(vr);
  
  /*---------------------------------------
   * Weaken the line accel affect
   *  k = fabsf(1.0f - norm)
   *  kp = _accel_kp * k/0.2f
   *---------------------------------------*/
  float norm = vm.norm();
  float line_accel = fabsf(1.0f - norm);
  
  if(line_accel > 0.2f)
  {
    line_accel = 0.2f;
  }
  
  float kp = _ka * (1 - line_accel / 0.22f);

  /*-------------------------------------
   * The angle needed to be correction
   *
   * |vc| = |vr| * |vm| * sin(theta)
   * theta = asin(|axis| / (|vr| * |vm|))
   * where |vr| = 1, |vm| = 1
   * so theta = asin(|vc|)
   *-------------------------------------*/
  norm = vc.normalize();
  if(norm > 1.0f) {norm = 1.0f;}
  float theta = asinf(norm);
  
  /*--------------------------------------
   * The correction quaternion
   *
   * dq = (cos(theta/2), vc*sin(theta/2))
   *--------------------------------------*/
  float correct_theta = 0.5f * theta * kp * dt;
  vc = vc*sinf(correct_theta);
  Quaternion dq(cosf(correct_theta), vc);
  dq.normalize();

  /*-------------
   * correct q
   *-------------*/
  Quaternion qe = q;
  q = dq*q;
  q.normalize();
  
  /*-------------------------------------------------
   *  correct gyro bias
   *  q = dq*qe
   *  dq = [1, -bx*dt*0.5, -by*dt*0.5, -bz*dt*0.5]
   *     = q*qe.conjugate()
   *  bx = -dq.q1*2/dt   
   *  by = -dq.q2*2/dt   
   *  bz = -dq.q3*2/dt
   *  Integral with LPF
   *-------------------------------------------------*/
  dq = q*qe.conjugate();
  Vector3f wb(-dq.q1*2, -dq.q2*2, 0);
  float wb_limit = 1.0f*DEG2RAD*dt;
  constrain(wb.x, -wb_limit, wb_limit);
  constrain(wb.y, -wb_limit, wb_limit);
  _w_b += wb*_kg;
}
/*
*/
void CPF_Att::correct_with_magn(Vector3f magn, float dt)
{
  
}



